#include "stdafx.h"
#include "PhysicsComponent.h"

PhysicsComponent::PhysicsComponent() {
	m_fresh = true;

	m_rigidBody = NULL;
	m_offset = Ogre::Vector3(0,0,0);
	m_updateOrientation = true;
}

PhysicsComponent::~PhysicsComponent() { }

bool PhysicsComponent::shouldUpdateOrientation() {
	return m_updateOrientation;
}

void PhysicsComponent::addToWorld(PhysicsSystem* hu) {
	m_fresh = false;
	
	// Add the rigid body to the physics world
	hu->getWorld()->lock();
	hu->getWorld()->addEntity(m_rigidBody);

	m_rigidBody->removeReference();
	
	hu->getWorld()->unlock();
}

void PhysicsComponent::setOffset(Ogre::Vector3 offset) {
	m_offset = offset;
}

bool PhysicsComponent::isFresh() {
	return m_fresh;
}

Ogre::Quaternion PhysicsComponent::getOrientation() {
	hkQuaternion quat = m_rigidBody->getRotation();
	hkVector4 imgpart = quat.getImag();
	hkReal realpart = quat.getReal();

	Ogre::Quaternion q = Ogre::Quaternion(realpart,imgpart(0),imgpart(1),imgpart(2));
	return q;
}

void PhysicsComponent::synchPositionAndOrientation(Ogre::Vector3 position, Ogre::Quaternion orientation) {
	hkVector4 p = hkVector4(position.x, position.y, position.z);

	hkQuaternion q;
	q.set(orientation.x, orientation.y, orientation.z, orientation.w);
}

void PhysicsComponent::applyImpulse(Ogre::Vector3 impulse) {
	if(m_rigidBody) {
		m_rigidBody->getWorld()->lock();
		m_rigidBody->applyForce( 0.5f, hkVector4(impulse.x, impulse.y, impulse.z) );
		m_rigidBody->getWorld()->unlock();
	}
}

Ogre::Vector3 PhysicsComponent::getPosition() {
	hkVector4 hv = m_rigidBody->getPosition();
	Ogre::Vector3 v(hv(0),hv(1),hv(2));

	return v+m_offset;
}

void PhysicsComponent::setGravityTowards(float deltaTime, Ogre::Vector3 point) {
	if(m_fresh || deltaTime > 3) {
		return;
	}

	Ogre::Vector3 position = getPosition();
	Ogre::Vector3 gravity = (point-position).normalisedCopy() * 64 * m_rigidBody->getMass();

	m_rigidBody->getWorld()->lock();
	m_rigidBody->applyForce( deltaTime, hkVector4(gravity.x, gravity.y, gravity.z) );
	m_rigidBody->getWorld()->unlock();
}